// MainDlg.cpp : implementation file
//

#include "stdafx.h"
#include "fluidApp.h"
#include "MainDlg.h"
#include ".\maindlg.h"
#include "ConfigManager.h"
#include "SimulatorWrapper.h"
#include "RenderType.h"
#include "Scene.h"
#include "Camera.h"
#include "StaticMesh.h"
#include "NavierStokesSolver.h"
#ifdef _SLC
#include "slcSurface.h"
#else
//#include "ParticleLevelSet.h"
#endif
#include "RenderMiscs.h"
//#include "MarchingCube.h"
#include "OpenGLRenderInterface.h"
#include "DecoLogger.h"
#include "timer.h"
#include "SnapshotManager.h"
#include "Stat.h"
//#include "densityField.h"
#include "PanelManager.h"
#include "tetVisualizer.h"
#include "ParticleSys.h"
#include "SceneBuilder.h"
#include "DecoArticulatedBody.h"
#include "SineWarp.h"
#include "CMASearcher.h"
#include "CMASearchContinuation.h"
#include "SearcherConfig.h"
const char* picDir = "..\\tmpPic\\";
const INT pathLen = 128;
BOOL g_bDumpObj = FALSE;
BOOL g_bDumpRib = FALSE;
BOOL g_bDumpDat = FALSE;
BOOL g_bDumpSolidRib = FALSE;
BOOL g_bDumpGridRib = FALSE;
BOOL g_bAnimationStart = FALSE;
BOOL g_bDumpParticleTrace = FALSE;
INT g_frameCounter = 0;
SimulatorWrapper* simulator = NULL;
// CMainDlg dialog

IMPLEMENT_DYNAMIC(CMainDlg, CDialog)
CMainDlg::CMainDlg(CWnd* pParent /*=NULL*/)
	: CDialog(CMainDlg::IDD, pParent)
{
}

CMainDlg::~CMainDlg()
{
}

void CMainDlg::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
}


BEGIN_MESSAGE_MAP(CMainDlg, CDialog)
	ON_WM_CREATE()
	ON_WM_DESTROY()
	ON_WM_ERASEBKGND()
	ON_WM_PAINT()
	ON_WM_LBUTTONDOWN()
	ON_WM_LBUTTONUP()
	ON_WM_RBUTTONDOWN()
	ON_WM_RBUTTONUP()
	ON_WM_MOUSEMOVE()
	ON_WM_MOUSEWHEEL()
	ON_WM_KEYDOWN()
	ON_WM_KEYUP()
	ON_WM_TIMER()
END_MESSAGE_MAP()

BOOL CMainDlg::SetupPixelFormat()
{
	static PIXELFORMATDESCRIPTOR pfd = 
	{
		sizeof(PIXELFORMATDESCRIPTOR),    // size of this pfd
			1,                                                         // version number
			PFD_DRAW_TO_WINDOW |                // support window
			PFD_SUPPORT_OPENGL |                   // support OpenGL
			PFD_DOUBLEBUFFER,                        // double buffered
			PFD_TYPE_RGBA,                               // RGBA type
			24,                                                       // 24-bit color depth
			0, 0, 0, 0, 0, 0,                                      // color bits ignored
			0,                                                         // no alpha buffer
			0,                                                         // shift bit ignored
			0,                                                         // no accumulation buffer
			0, 0, 0, 0,                                              // accumulation bits ignored
			16,                                                       // 16-bit z-buffer
			8,                                                         // 8-bit stencil buffer
			0,                                                         // no auxiliary buffer
			PFD_MAIN_PLANE,                              // main layer
			0,                                                         // reserved
			0, 0, 0                                                  // layer masks ignored
	};
	int m_nPixelFormat = ::ChoosePixelFormat(hdc, &pfd);
	if ( m_nPixelFormat == 0 )
	{
		return FALSE;
	}
	if ( ::SetPixelFormat(hdc, m_nPixelFormat, &pfd) == FALSE)
	{
		return FALSE;
	}
	return TRUE;
}

void CMainDlg::Init()
{
	CRect winrect;
	GetWindowRect(&winrect);
	win_width = winrect.Width();
	win_height = winrect.Height();
	DecoConfig::GetSingleton()->Init();

	scene = new DecoScene();
	DecoConfig::GetSingleton()->GetInt("DumpFile", "dat", g_bDumpDat);
	DecoConfig::GetSingleton()->GetInt("DumpFile", "obj", g_bDumpObj);
	DecoConfig::GetSingleton()->GetInt("DumpFile", "trace", g_bDumpParticleTrace);
	DecoConfig::GetSingleton()->GetInt("DumpFile", "rib", g_bDumpRib);
	DecoConfig::GetSingleton()->GetInt("DumpFile", "solidRib", g_bDumpSolidRib);
	DecoConfig::GetSingleton()->GetInt("DumpFile", "gridRib", g_bDumpGridRib);
	INT bUseSceneBuilderFile = 0;
	DecoConfig::GetSingleton()->GetInt("Scene", "useSceneBuilder", bUseSceneBuilderFile);

	string snapShotName;
	if (DecoConfig::GetSingleton()->GetString("StartFromSnapShot", "name", snapShotName))
	{
		char fullPath[100];
		string snapShotPath;
		DecoConfig::GetSingleton()->GetString("DumpPath", "dat", snapShotPath);
		sprintf(fullPath, "%s%s", snapShotPath.c_str(), snapShotName.c_str());
		DecoSnapshotManager::GetSingleton()->RecoverFromSnapshot(fullPath, scene);
	}
	else
	{
		INT maxLevel = 6, xElements = 64, xElementsDetail = 64, xElementsRho = 64;
		DecoConfig::GetSingleton()->GetInt("Resolution", "slcLevel", maxLevel);
		DecoConfig::GetSingleton()->GetInt("Resolution", "xElementsRho", xElementsRho);
		DecoConfig::GetSingleton()->GetInt("Resolution", "xElements", xElements);
		DecoConfig::GetSingleton()->GetInt("Resolution", "xElementsDetail", xElementsDetail);
		vector3 minPt(-1, -1, -1);
		vector3 maxPt(1, 1, 1);
		DecoConfig::GetSingleton()->GetVector3("Scene", "min", minPt);
		DecoConfig::GetSingleton()->GetVector3("Scene", "max", maxPt);
		Box scnBox(minPt, maxPt);
#ifdef _SLC
		SLC::GetSingleton()->SetMaxLevel(maxLevel);
#endif
#ifdef _DENSITY
		DensityField::GetSingleton()->SetSceneBox(scnBox);
		DensityField::GetSingleton()->SetNumElement(xElementsRho);
#endif
		g_CM = scene->camera;

		if (bUseSceneBuilderFile)
		{
			SceneBuilder::GetSingleton()->ConstructScene(scene);
			SceneBuilder::GetSingleton()->ConstructSceneWithoutDestroy(scene);
		}
		else
		{
			scene->SetSceneBox(scnBox);
			scene->SetNumXElement(xElements);
			scene->AllocateTempBuffer();
			scene->SetGridsInformation();
		}

		g_CM->setFrustum(45, static_cast<FLOAT>(win_width) / win_height, static_cast<FLOAT>(DEFAULT_NEAR_PLANE), DEFAULT_FAR_PLANE);
		g_CM->set(vector3(0,0, INIT_CAMERA), vector3(0, 0, 0), vector3(0, 1, 0));
	}

	scene->Reshape(win_width, win_height);

	bViewModel = true;
	bViewGrid = false;
	DecoRenderMisc::GetSingleton()->ShowBox(TRUE);
	uiTool.init(scene);
}

// CMainDlg message handlers

int CMainDlg::OnCreate(LPCREATESTRUCT lpCreateStruct)
{
	if (CDialog::OnCreate(lpCreateStruct) == -1)
		return -1;

	// TODO:  Add your specialized creation code here
	hdc = GetDC()->GetSafeHdc();
	if( !SetupPixelFormat() )
	{
		AfxMessageBox("Error in SetupPixel");
		return -1;
	}
	hrc = ::wglCreateContext(hdc);
	if(hrc == 0)
	{
		AfxMessageBox("Error in wglCreateContext");
		return -1;
	}
	wglMakeCurrent(hdc, hrc);
	Init();

	return 0;
}

void CMainDlg::OnDestroy()
{
	CDialog::OnDestroy();
	delete scene;
#ifdef _SLC
	SLC::GetSingleton()->DestroySingleton();
#endif
#ifdef _DENSITY
	DensityField::GetSingleton()->DestroySingleton();
#endif

	//MarchingCube::GetSingleton()->DestroySingleton();
	DecoLogger::GetSingleton()->DestroySingleton();
	DecoConfig::GetSingleton()->DestroySingleton();

	// TODO: Add your message handler code here
}

BOOL CMainDlg::OnEraseBkgnd(CDC* pDC)
{
	// TODO: Add your message handler code here and/or call default

	return true;//CDialog::OnEraseBkgnd(pDC);
}

void CMainDlg::OnPaint()
{
	CPaintDC dc(this); // device context for painting
	// TODO: Add your message handler code here
	// Do not call CDialog::OnPaint() for painting messages
	scene->RI->Clear();

	// Render background
	{
		INT x, y, widith, height;
		scene->RI->GetViewport(x, y, widith, height);

		scene->RI->shader->Bind();

		glDisable(GL_DEPTH_TEST);
		glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);

		glBegin(GL_TRIANGLE_STRIP);

		glVertex2i(-1, -1);
		glVertex2i(1, -1);
		glVertex2i(-1, 1);
		glVertex2i(1, 1);

		glEnd();

		scene->RI->shader->Unbind();

		glEnable(GL_DEPTH_TEST);
	}

	// Draw forces
	{
		vector<DecoSceneObject*> sceneObjs;
		scene->GetAllSceneObjects(sceneObjs);
		DecoSkelMesh* pMesh = (DecoSkelMesh*)sceneObjs[0]->GetRenderData();

		//glDisable(GL_DEPTH_TEST);
		glLineWidth(4.0f);

		glMatrixMode(GL_MODELVIEW);
		glLoadIdentity();
		glTranslatef(pMesh->mCOM.x, pMesh->mCOM.y, pMesh->mCOM.z);

		vector3 o = scene->camera->getCoords().origin();
		vector3 x, y, z;
		scene->camera->getCoords().GetBase(x, y, z);
		vector3 d = o - z * 10;
		gluLookAt(o.x, o.y, o.z, d.x, d.y, d.z, 0.0, 1.0, 0.0);


		glBegin(GL_LINES);

		const float fScl = 0.005f;

		glColor3f(1.0f, 1.0f, 1.0f);
		for(int i = 0; i < 5; i++)
		{
			glVertex3f(scene->mvCOMs[i].x, -scene->mvCOMs[i].z, scene->mvCOMs[i].y);
			glVertex3f(scene->mvCOMs[i].x + fScl * scene->mvBodyForces[i].x, -scene->mvCOMs[i].z - fScl * scene->mvBodyForces[i].z, scene->mvCOMs[i].y + fScl * scene->mvBodyForces[i].y);
		}
		glColor3f(0.6f, 0.9f, 0.6f);
		glVertex3f(scene->mvCOMs[5].x, -scene->mvCOMs[5].z, scene->mvCOMs[5].y);
		glVertex3f(scene->mvCOMs[5].x + fScl * scene->mvTorque.x, -scene->mvCOMs[5].z - fScl * scene->mvTorque.z, scene->mvCOMs[5].y + fScl * scene->mvTorque.y);
		glEnd();

		//glEnable(GL_DEPTH_TEST);
		glLoadIdentity();

		INT x1, y1, widith, height;
		scene->RI->GetViewport(x1, y1, widith, height);

		glMatrixMode(GL_PROJECTION);
		glPushMatrix();
		glLoadIdentity();
		glOrtho(0, widith, 0, height, -1, 1);

		glDisable(GL_DEPTH_TEST);
		glBegin(GL_LINES);
		glVertex2f(widith / 2, 40);
		glVertex2f(widith / 2 + ((DecoArticulatedObject*)sceneObjs[0])->mRotTorque * 60000, 40);
		glEnd();
		glEnable(GL_DEPTH_TEST);

		glPopMatrix();
		glMatrixMode(GL_MODELVIEW);
		glLineWidth(1.0f);
	}

	
	char fileName[pathLen];
	memset(fileName, 0, pathLen * sizeof(char));
	sprintf(fileName, "..\\tmpPic\\fluid1%04d.jpg", g_frameCounter);

#ifdef _DENSITY
	if (g_bAnimationStart)
	{
		scene->RenderVolume(bViewModel);
	}
	else
	{
		if(bViewGrid)
			scene->RenderGridField();
		scene->Render(bViewModel);
	}
#else
	if(bViewGrid)  //tanjie
		scene->RenderGridField();
	scene->Render(bViewModel);


	{
		vector<DecoSceneObject*> sceneObjs;
		scene->GetAllSceneObjects(sceneObjs);
		DecoSkelMesh* pMesh = (DecoSkelMesh*)sceneObjs[0]->GetRenderData();

		glDisable(GL_DEPTH_TEST);
		glLineWidth(4.0f);

		glMatrixMode(GL_MODELVIEW);
		glPushMatrix();
		glLoadIdentity();
		glTranslatef(pMesh->mCOM.x, pMesh->mCOM.y, pMesh->mCOM.z);

		vector3 o = scene->camera->getCoords().origin();
		vector3 x, y, z;
		scene->camera->getCoords().GetBase(x, y, z);
		vector3 d = o - z * 10;
		gluLookAt(o.x, o.y, o.z, d.x, d.y, d.z, 0.0, 1.0, 0.0);


		glBegin(GL_LINES);

		const float fScl = 0.005f;

		glColor3f(0.6f, 0.9f, 0.6f);
		glVertex3f(scene->mvCOMs[5].x, -scene->mvCOMs[5].z, scene->mvCOMs[5].y);
		glVertex3f(scene->mvCOMs[5].x + fScl * scene->mvTorque.x, -scene->mvCOMs[5].z - fScl * scene->mvTorque.z, scene->mvCOMs[5].y + fScl * scene->mvTorque.y);
		glEnd();

		glEnable(GL_DEPTH_TEST);
		glLineWidth(1.0f);
		glPopMatrix();
	}

	// Draw wing tip
	{
		glMatrixMode(GL_MODELVIEW);
		glPushMatrix();
		glLoadIdentity();

		vector3 o = scene->camera->getCoords().origin();
		vector3 x, y, z;
		scene->camera->getCoords().GetBase(x, y, z);
		vector3 d = o - z * 10;
		gluLookAt(o.x, o.y, o.z, d.x, d.y, d.z, 0.0, 1.0, 0.0);
		glDisable(GL_DEPTH_TEST);
		glLineWidth(4.0f);

		glColor3f(1.0f, 1.0f, 1.0f);
		glBegin(GL_LINE_STRIP);

		for(int i = 0; i < scene->mvTipTrace.size(); i++)
		{
			const vector4& vert = scene->mvTipTrace[i];
			glVertex3f(vert.x, -vert.z, vert.y);
		}

		glEnd();
		glBegin(GL_LINE_STRIP);

		for(int i = 0; i < scene->mvTipTraceBack.size(); i++)
		{
			const vector4& vert = scene->mvTipTraceBack[i];
			glVertex3f(vert.x, -vert.z, vert.y);
		}

		glEnd();

		glEnable(GL_DEPTH_TEST);
		glLineWidth(1.0f);
		glPopMatrix();
	}
#endif
	if(g_frameCounter > 0)
		scene->RI->SaveToImage(fileName);

	SwapBuffers(wglGetCurrentDC());

}

void CMainDlg::ToggleModelView()
{
	bViewModel = !bViewModel;
	Invalidate(FALSE);
}

void CMainDlg::OnLButtonDown(UINT nFlags, CPoint point)
{
	// TODO: Add your message handler code here and/or call default
	SetFocus();
    SetCapture();
	DecoSceneObject* prevFocus = uiTool.GetFocusedObj();
	if(uiTool.onLButtonDown((FLOAT)point.x, (FLOAT)win_height-point.y))
		Invalidate(FALSE);
	CDialog::OnLButtonDown(nFlags, point);
}

void CMainDlg::OnLButtonUp(UINT nFlags, CPoint point)
{
	// TODO: Add your message handler code here and/or call default
	// TODO: Add your message handler code here and/or call default
	SetFocus();
	ReleaseCapture();
	DecoSceneObject* prevFocus = uiTool.GetFocusedObj();
	if(uiTool.onLButtonUp((FLOAT)point.x, (FLOAT)win_height-point.y))
		Invalidate(FALSE);
	CDialog::OnLButtonUp(nFlags, point);
}

void CMainDlg::OnRButtonDown(UINT nFlags, CPoint point)
{
	// TODO: Add your message handler code here and/or call default

	CDialog::OnRButtonDown(nFlags, point);
}

void CMainDlg::OnRButtonUp(UINT nFlags, CPoint point)
{
	// TODO: Add your message handler code here and/or call default

	CDialog::OnRButtonUp(nFlags, point);
}

void CMainDlg::OnMouseMove(UINT nFlags, CPoint point)
{
	// TODO: Add your message handler code here and/or call default
	if(uiTool.onMouseMove((FLOAT)point.x, (FLOAT)win_height-point.y))
		Invalidate(FALSE);
	CDialog::OnMouseMove(nFlags, point);
}

BOOL CMainDlg::OnMouseWheel(UINT nFlags, short zDelta, CPoint pt)
{
	if(uiTool.onMouseScroll(zDelta))
		Invalidate(FALSE);
	return CDialog::OnMouseWheel(nFlags, zDelta, pt);
}

void CMainDlg::OnKeyDown(UINT nChar, UINT nRepCnt, UINT nFlags)
{
	// TODO: Add your message handler code here and/or call default
	UINT keyChar = nChar + 32;

	if(uiTool.onKeyDown(keyChar))
		Invalidate(FALSE);
	CDialog::OnKeyDown(nChar, nRepCnt, nFlags);
}

#define SEARCH_DIM 16
void CMainDlg::OnKeyUp(UINT nChar, UINT nRepCnt, UINT nFlags)
{
	// TODO: Add your message handler code here and/or call default
	DecoTimer t;
	DOUBLE time;
	CMASearchContinuation* searcher = NULL;
	//double arg[SEARCH_DIM];
	//double minBoundary[SEARCH_DIM];
	//double maxBoundary[SEARCH_DIM];
	//vector4 argVec;
	//CMASearcher* searcher = NULL;
	switch(nChar + 32)
	{
	case 'm': //need modification tanjie
		//searcher = new CMASearcher(SEARCH_DIM);

		//minBoundary[0] = minBoundary[1] = minBoundary[2] = minBoundary[3] = -2.5;
		//maxBoundary[0] = maxBoundary[1] = maxBoundary[2] = maxBoundary[3] = 0;

		//minBoundary[4] = minBoundary[5] = minBoundary[6] = minBoundary[7] = 0;
		//maxBoundary[4] = maxBoundary[5] = maxBoundary[6] = maxBoundary[7] = 2.5;

		//minBoundary[8] = minBoundary[9] = minBoundary[10] = minBoundary[11] = -1.5;
		//maxBoundary[8] = maxBoundary[9] = maxBoundary[10] = maxBoundary[11] = 1.5;

		//minBoundary[12] = minBoundary[13] = minBoundary[14] = minBoundary[15] = 0;
		//maxBoundary[12] = maxBoundary[13] = maxBoundary[14] = maxBoundary[15] = 2.5;


		////minBoundary[0] = minBoundary[1] = minBoundary[2] = minBoundary[3] = 0.0; //amplitude
		////minBoundary[4] = minBoundary[5] = minBoundary[6] = 0; //phase change

		////maxBoundary[0] = maxBoundary[1] = 0.8;
		////maxBoundary[2] = maxBoundary[3] = 0.6;
		////maxBoundary[4] = maxBoundary[5] = maxBoundary[6] = 6.28;
		//
		//searcher->Search(minBoundary, maxBoundary, arg);
		//value = Evaluator(arg, scene);

		//delete searcher;

		searcher = new CMASearchContinuation();
		searcher->Search();
		delete searcher;
		break;
		t.startTimer();
		for (INT i = 0; i < 1; i++)
		{
			scene->Update(scene->GetTimeStep());

#ifdef _SLC
			try
			{		
			SLC::GetSingleton()->Evolve();
			}
			catch (CException* e)
			{
				(*DecoLogger::GetSingleton()) << "Catch exception in SLC" << "\n";

				char errorMsg[255];
				e->GetErrorMessage(errorMsg, 255);
				(*DecoLogger::GetSingleton()) << errorMsg << "\n";
			}

#elif _DENSITY
			DensityField::GetSingleton()->Evolve();
#endif
		}

		Invalidate(FALSE);
		time = t.getCurrentTime();
		(*DecoLogger::GetSingleton())<<"TOTAL TIME "<<time<<"\n";

		MessageBox("Finished", MB_OK);
		
		break;
	default:
		break;
	}
	if(uiTool.onKeyUp(nChar+32))
		Invalidate(FALSE);
	CDialog::OnKeyUp(nChar, nRepCnt, nFlags);
}
void CMainDlg::StartTimer()
{
	SetTimer(1, 0, 0);
}

void CMainDlg::ToogleGridView()
{
	bViewGrid = !bViewGrid;
	Invalidate(FALSE);
}

void BlendParams(const std::vector<double*>& paramsContainer, int turnSignal, int oldTurnSignal, double ratio, double* paramsBlended, int num)
{
	double clampedRatio = ratio;
	if (clampedRatio > 1.0)
		clampedRatio = 1.0;
	else if (clampedRatio < 0.0)
		clampedRatio = 0.0;
	for (int i = 0; i < num; ++i)
	{
		paramsBlended[i] = clampedRatio * paramsContainer[turnSignal][i] + (1.0 - clampedRatio) * paramsContainer[oldTurnSignal][i];
	}
}

int DetermineAction(const std::vector<double>& q)
{
	const double turnThreshold = 0.05;
	const double posThreshold = 0.05;
	if (q[1] >= 0 && q[5] >= 0|| q[1] <= 0 && q[5] <= 0)
		return 0;
	if (q[1] <= -posThreshold || q[5] >= turnThreshold)
	{
		return 1;

	}
	else if (q[1] >= posThreshold || q[5] <= -turnThreshold)
	{
		return 2;

	}
	else
	{
		return 0;
	}
}

void CMainDlg::OnTimer(UINT_PTR nIDEvent)
{
	// TODO: Add your message handler code here and/or call default
#ifdef _DEBUG_TIME
	g_frameCounter++;
	DecoTimer frameTime;
	frameTime.startTimer();
	g_bAnimationStart = TRUE;
	DOUBLE TIME_STEP = scene->GetTimeStep();
	INT numStepPerFrame = 1;//static_cast<INT>(1.0 / (30 * TIME_STEP));
	*(DecoLogger::GetSingleton()) << "------------------Frame " << g_frameCounter << " Began-------------------\n";
#endif

	//scene->DumpSolidGrid();
	//exit(0);

	DecoArticulatedObject* articulatedObj = scene->GetArticulatedObject();
//	vector<double> desiredq, desiredqdot;
//	articulatedObj->GetDesiredQandQDot(g_frameCounter * TIME_STEP, desiredq, desiredqdot);
//SineWarp warp(1, 0.3, 0.5, 1);
//
//
//ofstream ofile("dof.log");
//for (int i = 0; i < 100; ++i)
//{
//	double valueX = warp.EvaluateValue(i * 0.01);
//	ofile << valueX << " ";
//}
//ofile << std::endl;
//std::vector<double> knots;
//knots.resize(3);
//knots[0] = 0.9;
//knots[1] = 0.9;
//knots[2] = 0.25;
//warp.SetParametersGreg(knots[0], knots[1], knots[2]);
//for (int i = 0; i < 100; ++i)
//{
//
//	double valueY = warp.EvaluateValue(i * 0.01);
//	ofile << valueY << " ";
//}
//exit(0);


	//double paramsStraight[] = {0.627023,  -0.787008,  0, 4.461485, 0, 0, 0, 0};

	//double paramsRight[] = {0.627023,  -0.787008,  0, 4.461485, 0.620409, 0, 0.936719, 0};
	//double paramsLeft[] = {0.627023,  -0.787008,  0, 4.461485, 0, -0.620409,  0, -0.936719};
	//double paramsBlended[] = {0, 0, 0, 0, 0, 0, 0, 0};
	//std::vector<double*> paramsContainer;
	//paramsContainer.push_back(paramsStraight);
	//paramsContainer.push_back(paramsRight);
	//paramsContainer.push_back(paramsLeft);


	std::vector<double> q;
	//static int turnSignal = 0;
	//static int oldTurnSignal = 0;
	//static int transitionFrame = 0;
	//if (g_frameCounter % 33 == 0)
	//{
	//	oldTurnSignal = turnSignal;
	//	transitionFrame = g_frameCounter;
	//	articulatedObj->GetQ(q);
	//	turnSignal = DetermineAction(q);
	//	(*DecoLogger::GetSingleton()) << "Change signal from " << oldTurnSignal << " to " << turnSignal << "\n";
	//}
	//(*DecoLogger::GetSingleton()) << "Current signal is " << turnSignal << "\n";
	//int numBlendingFrames = 10;
	//double ratio = (static_cast<double>(g_frameCounter - transitionFrame)) / numBlendingFrames;
	//BlendParams(paramsContainer, turnSignal, oldTurnSignal, ratio, paramsBlended, info.mJointBounds.size());
	//(*DecoLogger::GetSingleton()) << "Blended params are : " << paramsBlended[0] <<  " " << paramsBlended[1] <<  " " << paramsBlended[2] <<  " " << paramsBlended[3] <<  " " << paramsBlended[4] <<  " " << paramsBlended[5] <<  " " << paramsBlended[6] << " " << paramsBlended[7] << "\n";
	//articulatedObj->ApplyMotion(info, paramsBlended, info.mJointBounds.size());

	const SearcherInfo& info = DecoConfig::GetSingleton()->GetSearcherInfo();
	double* params = new double[info.mJointBounds.size()];
	memset(params, 0, info.mJointBounds.size() * sizeof(double));
	for (size_t i = 0; i < info.mJointParams.size(); ++i)
	{
		params[i] = info.mJointParams[i];
	}
	articulatedObj->ApplyMotion(info, params, info.mJointBounds.size());


	if (info.mbEnergyPrune)
	{ 
		double energy = articulatedObj->GetCumulativeEnergyConsumption().x;
		if (energy > 1.5 * info.mEnergyBound)
		{
			if (params)
				delete[] params;
			double objFuncValue = energy * info.mSearchTimeLength / scene->GetCurrentTime();
			(*DecoLogger::GetSingleton()) << "Energy budget exceeded too much: " << objFuncValue << "\n";
		}
	}

	vector3 energyOld = articulatedObj->GetCumulativeEnergyConsumption();


// 	{
// 		std::vector<double> q;
// 		articulatedObj->GetQ(q);
// 		articulatedObj->mPosFirstCycle = vector3(q[0], q[1], q[2]);
// 		articulatedObj->mOriFirstCycle = vector3(q[3], q[4], q[5]);
// 
// 		DecoSkelMesh* pMesh = (DecoSkelMesh*)articulatedObj->GetRenderData();
// 		DecoStaticMesh* pBody = (DecoStaticMesh*)pMesh->GetRenderDataFromIthBody(0);
// 		vector4 vDir4 = pBody->GetLocalToWorld() * vector4(-1.0, 0.0, 0.0, 0.0);
// 		articulatedObj->mDirFirstCycle = vector3(vDir4.x, vDir4.y, vDir4.z);
// 		articulatedObj->mDirFirstCycle.normalize();
// 	}

	scene->Update(TIME_STEP);

// 	articulatedObj->GetQ(q);
// 	vector3 energy = articulatedObj->GetCumulativeEnergyConsumption();
// 	scene->mEnergy.push_back(energy.x - energyOld.x);
// 	scene->mTorqueAbs.push_back(energy.y - energyOld.y);
// 	scene->mTorqueSqr.push_back(energy.z - energyOld.z);
// 	double deviation = articulatedObj->GetCumulativeDeviation();
// 	double colVol = articulatedObj->GetCumulativeCollisionVolume();
// 	(*DecoLogger::GetSingleton()) << "Current Q: (" << q[0] << ", " << q[1] << ", " << q[2] << ") and energy consumption: (" <<  energy.x << " " << energy.y << " " << energy.z << ").\n";
// 	(*DecoLogger::GetSingleton()) << "Collision Volume is " << colVol << "\n";
// 	(*DecoLogger::GetSingleton()) << "Deviation is " << deviation << "\n";
// 	(*DecoLogger::GetSingleton()) << "Time spent on FluidSparseVector::SetValue() is  " << FluidSparseVector::msAccumTime1 << " " << FluidSparseVector::msAccumTime2 << " " << FluidSparseVector::msAccumTime3 << "\n";
// 	FluidSparseVector::msAccumTime1 = FluidSparseVector::msAccumTime2 = FluidSparseVector::msAccumTime3 = 0;
// 	int timeStepsPerFrame = static_cast<int>(0.03 / TIME_STEP);
// 	if (g_frameCounter % timeStepsPerFrame == 0)
// 	{
// 		char fileName[pathLen];
// 		if (g_bDumpDat)
// 		{
// 			string snapShotPath;
// 			DecoConfig::GetSingleton()->GetString("DumpPath", "dat", snapShotPath);
// 			memset(fileName, 0, pathLen * sizeof(char));
// 			sprintf(fileName, "%sdump%d.dat", snapShotPath.c_str(), g_frameCounter);
// 			DecoSnapshotManager::GetSingleton()->SnapshotToFile(fileName, scene);
// 		}
// 		//if (g_frameCounter == 2)
// 		//	scene->Destroy(scene->GetFirstObject());
// #ifdef _SLC
// 		if (g_bDumpObj)
// 		{
// 			string objPath;
// 			DecoConfig::GetSingleton()->GetString("DumpPath", "obj", objPath);
// 			memset(fileName, 0, pathLen * sizeof(char));
// 			sprintf(fileName, "%sfoo1%04d.obj", objPath.c_str(), g_frameCounter);
// 			SLC::GetSingleton()->DumpObjFiles(fileName);
// 		}
// #endif
// 		if (g_bDumpObj)
// 		{
// 			string objPath;
// 			DecoConfig::GetSingleton()->GetString("DumpPath", "obj", objPath);
// 			memset(fileName, 0, pathLen * sizeof(char));
// 			sprintf(fileName, "%sfoo1%04d.obj", "../ObjFile/", g_frameCounter);
// 			scene->DumpObjFiles(fileName);
// 		}
// 		if (g_bDumpRib)
// 		{
// 			string ribPath;
// 			DecoConfig::GetSingleton()->GetString("DumpPath", "rib", ribPath);
// 			memset(fileName, 0, pathLen * sizeof(char));
// 			sprintf(fileName, "%sfoo1%04d.rib", ribPath.c_str(), g_frameCounter);
// #ifdef _SLC
// 			SLC::GetSingleton()->DumpRibFiles(fileName);
// #elif _PARTICLES
// 			scene->particles.DumpRIB(fileName);
// #endif
// 		}
// 		if (g_bDumpSolidRib)
// 		{
// 			string ribPath;
// 			DecoConfig::GetSingleton()->GetString("DumpPath", "rib", ribPath);
// 			memset(fileName, 0, pathLen * sizeof(char));
// 			sprintf(fileName, "%sfoo1%04d", ribPath.c_str(), g_frameCounter);
// 			scene->DumpSolidRib(fileName);
// 		}
// 		if (g_bDumpParticleTrace)
// 		{
// #ifdef _PARTICLES
// 			string tracePath;
// 			DecoConfig::GetSingleton()->GetString("DumpPath", "trace", tracePath);
// 			memset(fileName, 0, pathLen * sizeof(char));
// 			sprintf(fileName, "%sfoo1%04d", tracePath.c_str(), g_frameCounter);
// 
// 			scene->particles.DumpParticleTraces(fileName);
// #endif
// 		}
// 	}
// 
// 
// 
// 
// #ifdef _DEBUG_TIME
// 	DOUBLE currentTime = frameTime.getCurrentTime();
// 	*(DecoLogger::GetSingleton()) << "Frame " << g_frameCounter << " spent " << (currentTime) << " seconds\n";
// 	*(DecoLogger::GetSingleton()) << "------------------Frame " << g_frameCounter << " End-------------------\n\n\n";
// #endif
// 
// 
// 	if(scene->GetCurrentTime() >= 2.0 && articulatedObj->mPosFirstCycle == vector3(-999, -999, -999))
// 	{
// 		std::vector<double> q;
// 		articulatedObj->GetQ(q);
// 		articulatedObj->mPosFirstCycle = vector3(q[0], q[1], q[2]);
// 		articulatedObj->mOriFirstCycle = vector3(q[3], q[4], q[5]);
// 
// 		DecoSkelMesh* pMesh = (DecoSkelMesh*)articulatedObj->GetRenderData();
// 		DecoStaticMesh* pBody = (DecoStaticMesh*)pMesh->GetRenderDataFromIthBody(0);
// 		vector4 vDir4 = pBody->GetLocalToWorld() * vector4(-1.0, 0.0, 0.0, 0.0);
// 		articulatedObj->mDirFirstCycle = vector3(vDir4.x, vDir4.y, vDir4.z);
// 		articulatedObj->mDirFirstCycle.normalize();
// 
// 		*(DecoLogger::GetSingleton()) << "1st cycle marked.\n";
// 	}
// 
// 	if (scene->GetCurrentTime() >= info.mSearchTimeLength || g_frameCounter >= 241)
// 	{
// 		double result;
// 
// 		double distTraveledInDir = 0.0;
// 		double devia = abs(q[1]) + 3.5 * abs(q[2]) + abs(q[3]) + 3.5 * abs(q[4]) + abs(q[5]);
// 
// 		if (energy.x >= info.mEnergyBound)
// 			result = energy.x;
// 		else if(!info.mbEvalSecondCycle)
// 			result = q[0] + 0.2 * devia + 500 * colVol;
// 		else
// 		{
// 			distTraveledInDir = -DotProduct(vector3(q[0], q[1], q[2]) - articulatedObj->mPosFirstCycle, articulatedObj->mDirFirstCycle);
// 			devia = abs(q[3] - articulatedObj->mOriFirstCycle.x) + 2.5 * abs(q[4] - articulatedObj->mOriFirstCycle.y) + abs(q[5] - articulatedObj->mOriFirstCycle.z);
// 
// 			result = distTraveledInDir + 0.33 * devia + 500 * colVol;
// 		}
// 
// 		float resultDetails[10];
// 		resultDetails[0] = result;
// 		for (int i = 0; i < 6; ++i)
// 			resultDetails[i + 1] = q[i];
// 		resultDetails[7] = distTraveledInDir;
// 		resultDetails[8] = 0.33 * devia;
// 
// 		*(DecoLogger::GetSingleton()) << "Simulation Ends, Obj func value: ";
// 		*(DecoLogger::GetSingleton()) << resultDetails[0] << " \n";
// 		*(DecoLogger::GetSingleton()) << "Distance and deviation score: \n";
// 		*(DecoLogger::GetSingleton()) << resultDetails[7] << " " << resultDetails[8] << "\n";
// 		*(DecoLogger::GetSingleton()) << "Qs: ";
// 		for(int i = 1; i < 7; i++)
// 			*(DecoLogger::GetSingleton()) << resultDetails[i] << "  ";
// 
// 		*(DecoLogger::GetSingleton()) << "\n";
// 
// 		// Print energy at each time step
// 		for(int i = 0; i < scene->mEnergy.size(); i++)
// 		{
// 			*(DecoLogger::GetSingleton()) << scene->mEnergy[i] <<"\t";
// 		}
// 		*(DecoLogger::GetSingleton()) << "\n";
// 		for(int i = 0; i < scene->mTorqueAbs.size(); i++)
// 		{
// 			*(DecoLogger::GetSingleton()) << scene->mTorqueAbs[i] <<"\t";
// 		}
// 		*(DecoLogger::GetSingleton()) << "\n";
// 		for(int i = 0; i < scene->mTorqueSqr.size(); i++)
// 		{
// 			*(DecoLogger::GetSingleton()) << scene->mTorqueSqr[i] <<"\t";
// 		}
// 		*(DecoLogger::GetSingleton()) << "\n";
// 
// 		exit(0);
// 	}

	std::vector<double> qDot;
	articulatedObj->GetQDot(qDot);
	*(DecoLogger::GetSingleton()) << g_frameCounter << " " << qDot[0] << "\n";

	Invalidate(FALSE);
	CDialog::OnTimer(nIDEvent);
}
